﻿#include <glog/logging.h>
#include <algorithm>
#include <QDateTime>
#include <opencv2/opencv.hpp>

#include "constant.h"
#include "util/utils.h"
#include "detect/detector_thread.h"
#include "detect/result_manager.h"
#include "manager/datakeeper.h"
#include "manager/config_manager.h"

bool result_compare(AlgorithmResultEntity e1, AlgorithmResultEntity e2) {
    if (e1.xcenter > e2.xcenter) {
        return true;
    } else {
        return false;
    }
}


DetectorThread::DetectorThread(const string& model_file, const string& weights_file,
                               const string& mean_file, const string& mean_value)
{
    this->model_file = model_file;
    this->weights_file = weights_file;
    this->mean_file = mean_file;
    this->mean_value = mean_value;
    this->detector = NULL;
    moveToThread(this);
}

DetectorThread::~DetectorThread() {
    if (detector != NULL) {
        delete detector;
        detector = NULL;
    }
}

void DetectorThread::run() {
    LOG(INFO) << "DetectorThread | run";
    detector = new Detector(model_file, weights_file, mean_file, mean_value);
    exec();
}

void DetectorThread::on_detect(QSharedPointer<Mat> sp_frame) {
    if (detector != NULL) {
        LOG(INFO) << "DetectorThread | on_detect"
                  << QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss:zzz").toStdString();
        std::vector<AlgorithmResultEntity> frame_results;
        std::vector<vector<float> > detections = detector->Detect(*sp_frame);

        AppConfig *ptr_app_config = ConfigManager::getInstance()->get_app_config();
        if (ptr_app_config->algorithm_type == ALGORITHM_TYPE_VECHICLE_PEDESTRIAN) {
            for (int i = 0; i < detections.size(); ++i) {
                vector<float>& d = detections[i];
                if (d[2] >= ptr_app_config->confidence && d[1] == 7) {
                    AlgorithmResultEntity result = get_result_entity(d, sp_frame);
                    frame_results.push_back(result);
                }
            }
            update_result(frame_results, sp_frame, ptr_app_config);
        } else { 
            for (int i = 0; i < detections.size(); ++i) {
                vector<float>& d = detections[i];
                if (d[2] >= ptr_app_config->confidence && d[1] == 15) {
                    AlgorithmResultEntity result = get_result_entity(d, sp_frame);
                    frame_results.push_back(result);
                }
            }
            /* 未检测到行人时, 发送空的数据 */
            if (frame_results.size() >= 0) {
                 ResultManager::get_instance()->detect(sp_frame, frame_results);
            }
            emit sig_result(sp_frame);
        }
    }
}

AlgorithmResultEntity DetectorThread::get_result_entity(vector<float> &ssd_result, QSharedPointer<Mat> sp_frame) {
    // Detection format: [image_id, label, score, xmin, ymin, xmax, ymax].
    AlgorithmResultEntity result;
    result.image_id = ssd_result[0];
    result.label = ssd_result[1];
    result.score = ssd_result[2];

    result.xmin = std::max((int)(ssd_result[3] * sp_frame->cols), 0);
    result.ymin = std::max((int)(ssd_result[4] * sp_frame->rows), 0);
    result.xmax = std::min((int)(ssd_result[5] * sp_frame->cols), sp_frame->cols);
    result.ymax = std::min((int)(ssd_result[6] * sp_frame->rows), sp_frame->rows);

    result.width = result.xmax - result.xmin;
    result.height = result.ymax - result.ymin;
    result.xcenter = (result.xmax + result.xmin) / 2;
    result.ycenter = (result.ymax + result.ymin) / 2;
    result.xfooter = (result.xmax + result.xmin) / 2.0;
    result.yfooter = result.ymax;
    result.area = result.width * result.height;

    result.color = 0;
    result.orientation = 0;
    result.speed = 0;
    result.mass = 0;

    result.cali_x = 0;
    result.cali_y = 0;

    result.result_id = 0;
    result.is_match = false;

    result.sp_tracker = QSharedPointer<KCFTracker>(new KCFTracker(true, false, true, false));
    result.timestamp = QDateTime::currentMSecsSinceEpoch();

    result.kcf_roi = cv::Rect(result.xmin, result.ymin, result.width, result.height);
    result.kcf_ssd_roi = result.kcf_roi;

    if (Utils::is_detect_pedestrian() == true) {
        result.sp_tracker->init(result.kcf_roi, *sp_frame);
    }

    return result;
}

 void DetectorThread::update_result(std::vector<AlgorithmResultEntity> algorithm_results, QSharedPointer<Mat> sp_frame,
                                    AppConfig *app_config) {
    static cv::Mat similarity;
    ResultEntity *result_entity = DataKeeper::get_instance()->get_result();
    std::vector<ObjectEntity> objects;
    // 进行测量的统计
    if (DataKeeper::get_instance()->check_last_frame() == true) {
        // 获取上次原始的灰度图
        FrameResultEntity *last_frame_result = DataKeeper::get_instance()->get_last_frame_result();
        cv::Mat last_frame_gray;
        cv::cvtColor(*(last_frame_result->sp_frame), last_frame_gray, CV_RGB2GRAY);
        // 对检测结果进行匹配
        for (std::vector<AlgorithmResultEntity>::iterator it = algorithm_results.begin(); it != algorithm_results.end(); ++it) {
            double mag_r = 0.0;
            bool found = false;
            try {
                // 对识别的物体进行灰度操作
                cv::Mat roi(*sp_frame, cv::Rect(it->xmin, it->ymin, it->width, it->height));
                cv::Mat roi_gray;
                cv::cvtColor(roi, roi_gray, CV_RGB2GRAY);
                // 匹配检测的结果
                cv::matchTemplate(last_frame_gray, roi_gray, similarity, CV_TM_CCOEFF_NORMED);
                cv::minMaxLoc(similarity, 0, &mag_r, 0, NULL);
            } catch(...) {
                LOG(ERROR) << "ResultManager | update_result | exception"
                           <<it->xmin << "," << it->ymin << "," << it->width << "," << it->height;
            }
            if (mag_r < 0.95) {
                result_entity->vehicle_num++;
                found  = false;
            } else {
                found = true;
            }
            LOG(INFO) << "ResultManager | update_result | "
                      <<it->xmin << "," << it->ymin << "," << it->width << "," << it->height
                      << ", mag_r:" << mag_r << ", vehicle_num:" << result_entity->vehicle_num;
            if (app_config->show_detect == true) {
                ObjectEntity object;
                object.x = it->xmin;
                object.y = it->ymin;
                object.width = it->width;
                object.height = it->height;
                object.found = found;
                object.id = result_entity->vehicle_num;
                objects.push_back(object);
            }
        }
    } else {
        // 首次检测时直接累加车辆的检测结果
        for (std::vector<AlgorithmResultEntity>::iterator it = algorithm_results.begin(); it != algorithm_results.end(); ++it) {
            result_entity->vehicle_num++;
            LOG(INFO) << "ResultManager | update_result | "
                      << it->xmin << "," << it->ymin << "," << it->width << "," << it->height
                      << ", vehicle_num:" << result_entity->vehicle_num;;
            if (app_config->show_detect == true) {
                ObjectEntity object;
                object.x = it->xmin;
                object.y = it->ymin;
                object.width = it->width;
                object.height = it->height;
                object.found = false;
                object.id = result_entity->vehicle_num;
                objects.push_back(object);
            }
        }
    }
    FrameResultEntity frame_results;
    frame_results.sp_frame = sp_frame;
    DataKeeper::get_instance()->set_last_frame_result(frame_results);

    emit sig_result_detail(sp_frame, objects);
 }
